#ifndef UPDATE_H
#define UPDATE_H

// #include"my_controller_Luntui.h"

// void chassisR_feedback_update(chassis_t *chassis,vmc_leg_t *vmc_leg,const double *angular_velocity,const double *data);

float averr[10] = {0.0f};
/*
计算left_len  和 left_len-dot
*/
void chassisR_feedback_update(chassis_t *chassis, vmc_leg_t *vmc_leg, const double *angular_velocity, const double *data)
{
	// printf("Roll: %.3f rad, Pitch: %.3f rad, Yaw: %.3f rad\n",
	// data[0], data[1], data[2]);
	chassis->myPithR = data[1];
	chassis->myPithGyroR = 0.0f + angular_velocity[1];
	chassis->myRollGyroR = angular_velocity[0];

	chassis->total_yaw = data[2];
	// chassis->v_act=((chassis->wheel_motor[1].vel+chassis->wheel_motor[0].vel)/2.0f)*0.03375f;//0.03375m是轮子半径
	chassis->v = ((chassis->wheel_motor[1].vel - chassis->wheel_motor[0].vel) / 2.0f) * 0.03375f; // 0.03375m是轮子半径  + -
	// printf("r_len_dot: %.3f --l_len_dot: %.3f \n", chassis->wheel_motor[1].vel, chassis->wheel_motor[0].vel);

	chassis->x = chassis->x + chassis->v * 0.005f; // 控制周期为1ms ？？？

	// 连杆1和连杆2近似等长
	// 轮子在竖直方向运动近似为一条直线
	vmc_leg->left_len = 2.0f * vmc_leg->left_l1 * cos(chassis->joint_motor[0].pos);									   // 余弦定理
	vmc_leg->left_len_dot = -2.0f * vmc_leg->left_l1 * sin(chassis->joint_motor[0].pos) * chassis->joint_motor[0].vel; // 直接求导

	vmc_leg->right_len = 2.0f * vmc_leg->right_l1 * cos(chassis->joint_motor[1].pos);									   // 余弦定理
	vmc_leg->right_len_dot = -2.0f * vmc_leg->right_l1 * sin(chassis->joint_motor[1].pos) * (chassis->joint_motor[1].vel); // 直接求导

	if (data[0] > 0) // imu需要反装
	{
		chassis->myRoll = data[0] - 3.1415926f;
	}
	else if (data[0] <= 0)
	{
		chassis->myRoll = data[0] + 3.1415926f;
	}

	// if (data[1] < (3.1415926f / 4.0f) && data[1] > (-3.1415926f / 4.0f))
	// { // 根据pitch角度判断倒地自起是否完成
	// 	chassis->recover_flag = 0;
	// }
}

#endif // UTILS_H